/*
 * ConjugatedGradient.cpp
 *
 *      Author: Jerome RD Soine and Christoph A Brand
 *      Institution: Schwarz goup, Institute for Theoretical Physics, Heidelberg University, Germany
 */

#include "../../../include/core/optimizer/ConjugatedGradient.h"

#include <sstream>

ConjugatedGradient::ConjugatedGradient(CellModel * CELL_MODEL, SubstrateModel * SUBSTRATE_MODEL, Comparator * COMPARATOR, string SOLUTION_FOLDER,unsigned int N_ITERATIONS, unsigned int N_THREADS, double TOLERANCE, double GRADIENT_SPACING_BASE)
: Optimizer (CELL_MODEL, SUBSTRATE_MODEL, COMPARATOR, SOLUTION_FOLDER, N_ITERATIONS, N_THREADS, TOLERANCE, GRADIENT_SPACING_BASE)
{
	current_CG_position = cell_model->get_parameters();
	current_CG_iteration_step = 0;
	initial_fitness=10;
}

void ConjugatedGradient::run()
{
	cout << "\n \n STARTING CG OPTIMIZATION \n \n";
	write_current_state(0);
	for(unsigned int i=1;i<n_iterations;i++)
	{
		iterate();
		write_current_state(i);
	}
}



void ConjugatedGradient::iterate()
{
	cout << "\n " << ++current_CG_iteration_step  << " ITERATE \n \n";


		/*
		 * Compute gradient
		 */
		cout << "Evaluate Gradient \n";
		vector<double> g = gradient(current_CG_position);

		vector<double> conjugated_gradient(g.size(),0);

		if(current_CG_iteration_step == 1){
			conjugated_gradient = g;
			last_conjugated_gradient = g;
			last_gradient=g;
		} else {
			double gammaZ = 0, gammaN=0, gamma=0;
			for (unsigned int i = 0; i < g.size(); i++) {
				gammaZ += (g[i]-last_gradient[i]) * g[i];
				gammaN += last_gradient[i]*last_gradient[i];
			}

			gamma = gammaZ/gammaN;

			for (unsigned int i = 0; i < g.size(); i++) {
				conjugated_gradient[i] = g[i] + gamma * last_conjugated_gradient[i];
			}
			last_gradient=g;
			last_conjugated_gradient=conjugated_gradient;
		}


		/*
		 * Apply line minimization along maximum change direction
		 */
		cout << "Apply LineMinimization \n";
		current_CG_position = line_minimization(current_CG_position, conjugated_gradient);
		solution = current_CG_position;
}


void ConjugatedGradient::write_current_state(unsigned int step_index)
{
			/*
			 * Write intermediate solution
			 */
			stringstream ss;
			ss << step_index;

			Worker * w = worker_manager->getWorker(solution, -1);
			w->work();
			w->get_substrate_model()->write_output(solution_folder + "/" + ss.str());
			write_convergence_file(w);
			write_FA_state_file(w);
			write_solution_to_file();
			w->unlock();
}

void ConjugatedGradient::write_convergence_file(Worker * w)
{
	ofstream out ((solution_folder + "/convergence.txt").c_str(),ios::app);
	if(current_CG_iteration_step==0)
	{
		initial_fitness = w->get_fitness();
		out << "# 1 iteration step <TAB> 2 fitness <TAB> 3 relative fitness <TAB> ComparisonValue <TAB> Regularization term with reg. param. <TAB> Regularization term w/o reg. param. <TAB> 4 total force [nN]" << "\n";
	}
	out << current_CG_iteration_step << "\t"
		<< w->get_fitness() << "\t"
		<< w->get_fitness()/initial_fitness << "\t"
		<< w->get_comparator()->getComparisonValue(w->get_substrate_model()) << "\t"
		<< w->get_comparator()->getRegularizationParameter() * w->get_comparator()->getRegularizationValue(w->get_substrate_model()) << "\t"
		<< w->get_comparator()->getRegularizationValue(w->get_substrate_model()) << "\t"
		<< w->get_cell_model()->get_total_force() << "\n";
	out.close();
}

void ConjugatedGradient::write_FA_state_file(Worker * w)
{
	stringstream ss;
	ss << current_CG_iteration_step;
	ofstream out ((solution_folder+"/"+ss.str()+"_FA_state.txt").c_str());
	out << "# FA index <TAB> FA_area <TAB> FA_orientation <TAB> F_X [nN] <TAB> F_Y [nN] <TAB> abs(F) [nN] \n";
    unsigned int n_FAs = w->get_cell_model()->get_force_transmissions().size();
    for(unsigned int i=0;i<n_FAs;i++)
    {
    	out << i << "\t";
    	out << w->get_cell_model()->get_force_transmissions()[i]->get_shape()->get_area() << "\t";
    	out << w->get_cell_model()->get_force_transmissions()[i]->get_shape()->get_orientation_angle() << "\t";
    	out << w->get_cell_model()->get_force_transmissions()[i]->get_force()[0] << "\t";
    	out << w->get_cell_model()->get_force_transmissions()[i]->get_force()[1] << "\t";
    	out << w->get_cell_model()->get_force_transmissions()[i]->get_force_magnitude() << "\n";
    }
    out.close();

}

void ConjugatedGradient::write_solution_to_file()
{
	stringstream ss;
	ss << current_CG_iteration_step;
	ofstream out ((solution_folder+"/"+ss.str()+"_solution.txt").c_str());
	for(unsigned int i=0;i<solution.size();i++)
		out << solution[i] << "\n";
	out.close();

}
